#include "usound-sample-v1.h"

namespace usound_sample_v1
{
	class USoundSampleVars : public SessionSampleVars
	{
	public:
		SPADAS_VARS(USoundSample, SessionSampleVars)
		USoundSampleData data;

		SessionGeneralSample toGeneralSample()
		{
			SessionGeneralSample sample;
			sample.timestamp() = timestamp;

			Array<GeneralElement> values(1 + 9 * data.dataList.size());
			sample.values() = values;

			values[0] = data.dataList.size();

			for (UInt i = 0; i < data.dataList.size(); i++)
			{
				USoundSensorData& src = data.dataList[i];
				values[1 + 9 * i] = src.id;
				values[2 + 9 * i] = src.distance;
				values[3 + 9 * i] = src.properties.originX;
				values[4 + 9 * i] = src.properties.originY;
				values[5 + 9 * i] = src.properties.orientation;
				values[6 + 9 * i] = src.properties.angleRangeAlpha;
				values[7 + 9 * i] = src.properties.angleRangeBeta;
				values[8 + 9 * i] = src.properties.distanceRangeAlpha;
				values[9 + 9 * i] = src.properties.distanceRangeBeta;
			}

			sample.significantCount() = values.size();
			return sample;
		}

		Bool fromSample(String protocol, SessionSample sample) override
		{
			if (!sample.is<SessionGeneralSample>()) return FALSE;

			auto generalSample = sample.as<SessionGeneralSample>();
			timestamp = generalSample.timestamp();
			auto values = generalSample.values();

			if (protocol == "usound-sample-v1")
			{
				if (values.size() < 1) return FALSE;
				if (!values[0].valid) return FALSE;

				UInt sensorCount = (UInt)values[0].value;
				if (values.size() != 1 + sensorCount * 9) return FALSE;

				data.dataList = Array<USoundSensorData>(sensorCount);
				for (UInt i = 0; i < sensorCount; i++)
				{
					USoundSensorData& sensor = data.dataList[i];
					UInt baseIndex = 1 + 9 * i;

					sensor.id = (Int)values[baseIndex].value;
					sensor.distance = values[baseIndex + 1];
					sensor.properties.originX = values[baseIndex + 2].value;
					sensor.properties.originY = values[baseIndex + 3].value;
					sensor.properties.orientation = values[baseIndex + 4].value;
					sensor.properties.angleRangeAlpha = values[baseIndex + 5].value;
					sensor.properties.angleRangeBeta = values[baseIndex + 6].value;
					sensor.properties.distanceRangeAlpha = values[baseIndex + 7].value;
					sensor.properties.distanceRangeBeta = values[baseIndex + 8].value;
				}

				return TRUE;
			}
			
			return FALSE;
		}
		
		Bool supportInterpolation() override
		{
			return TRUE;
		}

		SessionSample interpolate(SessionSample& ss1, Double w1, SessionSample& ss2, Double w2, FullTimestamp timestamp) override
		{
			if (!ss1.is<USoundSample>() || !ss2.is<USoundSample>()) return SessionSample();

			USoundSampleData& s1 = ss1.as<USoundSample>().getVars()->data;
			USoundSampleData& s2 = ss2.as<USoundSample>().getVars()->data;
			
			// 创建输出对象
			USoundSample output;
			output.timestamp() = timestamp;
			USoundSampleData& so = output.getVars()->data;
			
			// 合并传感器数据
			ArrayX<USoundSensorData> sensors;
			
			// 遍历第一个样本中的所有传感器数据
			for (auto sensor1 = s1.dataList.firstElem(); sensor1.valid(); ++sensor1)
			{
				// 在第二个样本中查找相同ID的传感器
				for (auto sensor2 = s2.dataList.firstElem(); sensor2.valid(); ++sensor2)
				{
					// 如果找到相同ID的传感器，则执行插值
					if (sensor1->id == sensor2->id)
					{
						USoundSensorData newSensor;
						newSensor.id = sensor1->id;
						
						// 对于Properties，直接使用第一个样本中的属性
						// 因为传感器的物理属性通常不会改变
						newSensor.properties.originX = sensor1->properties.originX;
						newSensor.properties.originY = sensor1->properties.originY;
						newSensor.properties.orientation = sensor1->properties.orientation;
						newSensor.properties.angleRangeAlpha = sensor1->properties.angleRangeAlpha;
						newSensor.properties.angleRangeBeta = sensor1->properties.angleRangeBeta;
						newSensor.properties.distanceRangeAlpha = sensor1->properties.distanceRangeAlpha;
						newSensor.properties.distanceRangeBeta = sensor1->properties.distanceRangeBeta;
						
						// 对于Distance，只有当两个样本中都有值时才进行插值
						// 否则设为无效值
						if (sensor1->distance.valid && sensor2->distance.valid)
						{
							newSensor.distance.valid = TRUE;
							newSensor.distance.value = sensor1->distance.value * w1 + sensor2->distance.value * w2;
						}
						else if (sensor1->distance.valid)
						{
							newSensor.distance = sensor1->distance;
						}
						else if (sensor2->distance.valid)
						{
							newSensor.distance = sensor2->distance;
						}
						
						sensors.append(newSensor);
						break;
					}
				}
			}
			
			so.dataList = sensors.toArray();
			return output.as<SessionSample>();
		}
	};
}

using namespace usound_sample_v1;

USoundSample::USoundSample() : Object<USoundSampleVars>(new USoundSampleVars, TRUE)
{}

FullTimestamp& USoundSample::timestamp()
{
	return vars->timestamp;
}

USoundSampleData *USoundSample::operator ->()
{
    return &vars->data;
}

String USoundSample::getProtocolName()
{
    return "usound-sample-v1";
}

SessionGeneralSample USoundSample::toGeneralSample()
{
    return vars->toGeneralSample();
}

Array<String> USoundSample::getProtocolNames()
{
	return Array<String>::create(1, "usound-sample-v1");
}

Bool USoundSample::fromGeneralSample(String protocol, SessionGeneralSample generalSample)
{
    return vars->fromSample(protocol, generalSample.as<SessionSample>());
}